#ifndef DSO_ENERGY_FUNCTIONAL_STRUCTS_H
#define DSO_ENERGY_FUNCTIONAL_STRUCTS_H

#include <vector>
#include <cmath>

#include "dso/util/NumType.h"
#include "dso/OptimizationBackend/RawResidualJacobian.h"


// EF stands for Energy Function
// 这个文件中的结构都是用来优化的，而真实的Frame, point在FullSystem/HessianBlocks里。
// 它们会从真实的Frame, point中拿取数据，进行优化，然后再将结果返回回去
// 所以这里的结构会持有frame, point, residual的指针，反过来，那些结构也会持有这里对象的指针

namespace dso 
{
    
// forward declare
class PointFrameResidual;
class CalibHessian;
class FrameHessian;
class PointHessian;
class EFResidual;
class EFPoint;
class EFFrame;
class EnergyFunctional;

// 在point-frame residual里用的energy
class EFResidual
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    inline EFResidual(PointFrameResidual* org, EFPoint* point_, EFFrame* host_, EFFrame* target_) :
        data(org), point(point_), host(host_), target(target_)
    {
        J = new RawResidualJacobian();
        assert(((long)this)%16==0);
        assert(((long)J)%16==0);
    }
    
    inline ~EFResidual()
    {
        delete J;
    }

    void takeDataF();
    void fixLinearizationF(EnergyFunctional* ef);
    inline const bool &isActive() const {return isActiveAndIsGoodNEW;}

    // structural pointers
    PointFrameResidual* data =nullptr;
    int hostIDX, targetIDX;
    EFPoint* point =nullptr;
    EFFrame* host =nullptr;
    EFFrame* target =nullptr;
    int idxInAll;

    RawResidualJacobian* J =nullptr;
    VecNRf res_toZeroF;
    Vec8f JpJdF;

    // status.
    bool isLinearized =false;

    // if residual is not OOB & not OUTLIER & should be used during accumulations
    bool isActiveAndIsGoodNEW =false;
};
    
// 在优化当中的一个Point
// Point 只有一个待估计参数：idepth

enum EFPointStatus {PS_GOOD=0, PS_MARGINALIZE, PS_DROP};

class EFPoint
{
public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EFPoint(PointHessian* d, EFFrame* host_) : data(d),host(host_)
    {
        takeData();
        stateFlag=EFPointStatus::PS_GOOD;
    }
    
    // take the date from PointHessian 
    void takeData();

    PointHessian* data; 
    
    float priorF; // 先验
    float deltaF; // 先验与0之差

    // constant info (never changes in-between).
    int idxInPoints;
    EFFrame* host;  // the host frame 

    // contains all residuals.
    std::vector<EFResidual*> residualsAll;

    // H and b blocks
    float bdSumF;
    float HdiF;
    float Hdd_accLF;
    VecCf Hcd_accLF;
    float bd_accLF;
    float Hdd_accAF;
    VecCf Hcd_accAF;
    float bd_accAF;

    EFPointStatus stateFlag;
};

// 在优化当中的一个Frame
class EFFrame
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EFFrame(FrameHessian* d) : data(d)
    {
        takeData();
    }
    void takeData();

    // Frame 状态量为八维：6自由度Pose+2自由度曝光
    Vec8 prior;             // prior hessian (diagonal)
    Vec8 delta_prior;       // = state-state_prior (E_prior = (delta_prior)' * diag(prior) * (delta_prior)
    Vec8 delta;             // state - state_zero.

    std::vector<EFPoint*> points;   // associated points
    FrameHessian* data; // real data 
    int idx;    // idx in frames.
    int frameID;
};

}


#endif
